//
// Created by jacy on 22-7-13.
//

#include<iostream>
#include "ros/ros.h"
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <geometry_msgs/Twist.h>
#include <Eigen/Core>
#include "sensor_msgs/Joy.h"
#include "mavros_msgs/SetMavFrame.h"
#include <opencv2/core/core.hpp>
#include "queue"
using namespace std;
class PID{
public:
    Eigen::Vector3d PIDControl(double Kp,double Ki,double max,double min,Eigen::Vector3d current_pos,Eigen::Vector3d target_pose){
        Eigen::Vector3d out;

        Eigen::Vector3d err;
        err =  target_pose - current_pos;

        err_sum += err;
        out = Kp*err+Ki*err_sum;

        for(int i = 0;i<3;i++){
            if(abs(out[i])<min) out[i] = min * out[i]/abs(out[i]);
            else if(abs(out[i]) > max) out[i] = max * out[i]/abs(out[i]);
        }
        return out;

    }
private:
    Eigen::Vector3d err_sum = {0,0,0};

};



static enum Mission {
    NONE,
    OFFBOARD,
    ARM,
    TAKEOFF,
    LAND,
    LINE,
    CIRCLE,
    TRAJECTORY,
    RECORD,
    POSTARGET
} mission_cmd=NONE;

string MISSIONCommandStr[] = {
    "NONE",
    "OFFBOARD",
    "ARM",
    "TAKEOFF",
    "LAND",
    "LINE",
    "CIRCLE",
    "TRAJECTORY",
    "RECORD",
    "POSTARGET"};

class Direction{
public:
    bool NONE = true;
    bool FORWARD= false;
    bool BACKWARD= false;
    bool LEFT= false;
    bool RIGHT= false;
    bool UP= false;
    bool DOWN = false;
    bool TURNLEFT= false;
    bool TURNRIGHT= false;
    void reset(){
        NONE = true;
        FORWARD= false;
        BACKWARD= false;
        LEFT= false;
        RIGHT= false;
        UP= false;
        DOWN = false;
        TURNLEFT= false;
        TURNRIGHT= false;
    }
    void check(){
        if(FORWARD && BACKWARD) FORWARD = false,BACKWARD = false;
        if(LEFT && RIGHT) LEFT = false,RIGHT = false;
        if(UP && DOWN) UP = false,DOWN = false;
        if(TURNLEFT && TURNRIGHT) TURNLEFT = false,TURNRIGHT = false;
    }
    string get_info(){
        string info = " ";
        if(FORWARD) info += "FORWARD+";
        if(BACKWARD) info += "BACKWARD+";
        if(LEFT) info += "LEFT+";
        if(RIGHT) info += "RIGHT+";
        if(UP) info += "UP+";
        if(DOWN) info += "DOWN+";
        if(TURNLEFT) info += "TURNLEFT+";
        if(TURNRIGHT) info += "TURNRIGHT+";
        info.pop_back();
        return info;
    }
} direction;
Eigen::Vector3d current_position;
geometry_msgs::PoseStamped pose;
mavros_msgs::State current_state;
double speed[2] = {0.8,1.2};
double angle[2] = {0.4,0.6};
bool fast_speed = false;
void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}
cv::RNG rng;
void local_odom_cb(const geometry_msgs::PoseStampedConstPtr& msg)
{
    current_position[0] = msg->pose.position.x + rng.gaussian(0.04);
    current_position[1] = msg->pose.position.y + rng.gaussian(0.04);
    current_position[2] = msg->pose.position.z + rng.gaussian(0.1);
}

void joy_cb(const sensor_msgs::Joy::ConstPtr& msg){
    if(msg->buttons[3] == 1) mission_cmd = OFFBOARD;
    else if(msg->buttons[2] == 1) mission_cmd = ARM;
    else if(msg->buttons[0] == 1) mission_cmd = TAKEOFF;
    else if(msg->buttons[1] == 1) mission_cmd = LAND;
    else if(msg->axes[7] == 1.0) mission_cmd = LINE;
    else if(msg->axes[6] == -1.0 ) mission_cmd = CIRCLE;
    else if(msg->axes[6] == 1.0 ) mission_cmd = TRAJECTORY;
    else if(msg->axes[7] == -1.0) mission_cmd = RECORD;
    else if(msg->axes[4] == 1.0) mission_cmd = POSTARGET;
    else mission_cmd = NONE;
    direction.reset();
    if(msg->axes[5] <-0.5) direction.UP = true , direction.NONE = false;
    if(msg->axes[2] <-0.5) direction.DOWN = true, direction.NONE = false;
    if(msg->buttons[5] == 1) direction.TURNRIGHT = true, direction.NONE = false;
    if(msg->buttons[4] == 1) direction.TURNLEFT = true, direction.NONE = false;
    if(msg->axes[1]>0.5) direction.FORWARD = true, direction.NONE = false;
    else if(msg->axes[1]<-0.5) direction.BACKWARD = true, direction.NONE = false;
    if(msg->axes[0] >0.5) direction.LEFT = true, direction.NONE = false;
    if(msg->axes[0] <-0.5) direction.RIGHT = true, direction.NONE = false;
    if(msg->buttons[10] == 1) fast_speed = true;
    else fast_speed = false;
    direction.check();

    string info = "";
    if(mission_cmd != NONE) info += "Mission: " + MISSIONCommandStr[mission_cmd];
    if(!direction.NONE) info+= "|| Direction: " + direction.get_info();
    if(fast_speed) info += "||fast ";
    cout<<info<<endl;
}

int main(int argc, char **argv)
{

    ros::init(argc, argv, "joy_control");
    ros::NodeHandle nh;

    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
            ("mavros/state", 10, state_cb);
    ros::Subscriber local_odom_sub = nh.subscribe<geometry_msgs::PoseStamped>
            ("/mavros/local_position/pose", 10, local_odom_cb);

    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
            ("mavros/setpoint_position/local", 10);
    ros::Publisher vel_pub = nh.advertise<geometry_msgs::Twist>("/mavros/setpoint_velocity/cmd_vel_unstamped",1);
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
            ("mavros/cmd/arming");
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
            ("mavros/set_mode");

    ros::ServiceClient set_frame = nh.serviceClient<mavros_msgs::SetMavFrame>
            ("/mavros/setpoint_velocity/mav_frame");

    ros::Subscriber joy_sub = nh.subscribe<sensor_msgs::Joy>("/joy", 1, joy_cb);

    //the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(50.0);

    // wait for FCU connection
    while(ros::ok() && !current_state.connected){
        std::cout<<"wait for FCU connection"<<std::endl;
        ros::spinOnce();
        rate.sleep();
    }

    pose.pose.position.x = 0;
    pose.pose.position.y = 0;
    pose.pose.position.z = 0;

    //send a few setpoints before starting
    for(int i = 80; ros::ok() && i > 0; --i){
        local_pos_pub.publish(pose);
        ros::spinOnce();
        rate.sleep();
    }

    ros::Time last_request = ros::Time::now();
    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";

    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;

    mavros_msgs::SetMavFrame service_call;
    service_call.request.mav_frame = 8;//BODY_FRAME
    while(!set_frame.call(service_call));

    cout<<"control!"<<endl;

    while (ros::ok())
    {
        geometry_msgs::Twist twist_msg = geometry_msgs::Twist();
        if(mission_cmd == NONE) ;
        else if(mission_cmd == OFFBOARD) {//off board
            if(current_state.mode == "OFFBOARD") ROS_INFO("Offboard enabled");
            else if (ros::Time::now() - last_request > ros::Duration(1.0)) {
                if (set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent)
                    ROS_INFO("Offboard enabled");
                last_request = ros::Time::now();
            }
        }
        else if(mission_cmd == ARM) {
            if(current_state.armed) ROS_INFO("Vehicle armed");
            else if(ros::Time::now() - last_request > ros::Duration(1.0)){
                if( arming_client.call(arm_cmd) &&
                    arm_cmd.response.success){
                    ROS_INFO("Vehicle armed");
                }
                last_request = ros::Time::now();
            }
        }
        else if(mission_cmd == TAKEOFF){
            //可打断
            twist_msg.linear.z = 1;
            for(int i=0; ros::ok()&& mission_cmd == TAKEOFF && i < 100; i++ ){
                vel_pub.publish(twist_msg);
                ros::spinOnce();
                rate.sleep();
            }
            twist_msg.linear.z = 0;
        }
        else if(mission_cmd == LAND){
            twist_msg.linear.z = -0.5;
            for(int i = 100; ros::ok() &&mission_cmd == LAND && i > 0; --i){
                vel_pub.publish(twist_msg);
                ros::spinOnce();
                rate.sleep();
            }
            twist_msg.linear.z = 0;
        }
        else if(mission_cmd == LINE){
            twist_msg.linear.x = 0.8;
            for(int i = 50; ros::ok() &&mission_cmd != LAND && i > 0; --i){
                vel_pub.publish(twist_msg);
                ros::spinOnce();
                rate.sleep();
            }
            twist_msg.linear.x = 0;
        }
        else if(mission_cmd == CIRCLE){
            //TODO
            twist_msg.linear.x = 1;
            twist_msg.angular.z = -0.6;
            for(int i = 3000; ros::ok() &&mission_cmd != LAND && i > 0; --i){
                vel_pub.publish(twist_msg);
                ros::spinOnce();
                rate.sleep();
            }
            twist_msg.linear.x = 0;
            twist_msg.angular.z = 0;
        }
        else if(mission_cmd == TRAJECTORY){
            //TODO
        }
        else if(mission_cmd == RECORD){
            //TODO
        }
        else if(mission_cmd == POSTARGET){
            Eigen::Vector3d target_position = {4,6,1.5};
            double kp = 0.5,ki = 0.05;
            deque<Eigen::Vector3d> current_pos_queue;
            while( mission_cmd != LAND )
            {
                if(abs((current_position - target_position).sum())<0.2 )
                {
                    if(current_pos_queue.size()>=10) current_pos_queue.pop_back();
                    current_pos_queue.push_front(current_position);
                    Eigen::Vector3d sum;
                    Eigen::Vector3d max = {current_position[0],current_position[1],current_position[2]};
                    for(int i = 0 ;i<current_pos_queue.size();i++){
                        sum += current_pos_queue[i];
                        if(abs((current_pos_queue[i] - target_position).sum())>abs((max - target_position).sum()))
                            max = current_pos_queue[i];
                    }
                    if(current_pos_queue.size()==10){
                        sum -= max;
                        sum /= 9;
                        current_position = sum;
                    }
                }

                geometry_msgs::Twist pos_target_vel;
                PID pid;
                Eigen::Vector3d vel = pid.PIDControl(kp,ki,2,0,current_position,target_position);
                pos_target_vel.linear.x = vel[0];
                pos_target_vel.linear.y = vel[1];
                pos_target_vel.linear.z = vel[2];
                vel_pub.publish(pos_target_vel);


                cout<<"err:"<<(current_position - target_position).sum()<<endl;
                ros::spinOnce();
                rate.sleep();
            }
        }
        if(mission_cmd == NONE){
            int mode = 0;
            if(fast_speed) mode = 1;//cout<<"fast"<<endl;

            twist_msg.linear.x = speed[mode] * direction.FORWARD - speed[mode] * direction.BACKWARD ;
            twist_msg.linear.y = -speed[mode] * direction.RIGHT + speed[mode] * direction.LEFT ;
            twist_msg.linear.z = speed[mode] * direction.UP - speed[mode] * direction.DOWN ;
            twist_msg.angular.z = -angle[mode] * direction.TURNRIGHT + angle[mode] * direction.TURNLEFT ;
        }

        vel_pub.publish(twist_msg);

        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}
